#include "quintic.h"
#include "math.h"


int qcCalcFactor(STRUCT_QUINTIC_PARM_I Pram, STRUCT_QUINTIC_FACTOR *DataO)
{
    float t1,t2,t3,t4,t5;
    t1 = Pram.Time;
    t2 = t1*t1;
    t3 = t2*t1;
    t4 = t3*t1;
    t5 = t4*t1;
    DataO->F = Pram.PointStart.q;
    DataO->E = Pram.PointStart.dq;
	DataO->D = Pram.PointStart.ddq * (float)0.5;
	DataO->C = ((float)20 * (Pram.PointEnd.q - Pram.PointStart.q) -
		((float)8 * Pram.PointEnd.dq + (float)12 * Pram.PointStart.dq)*t1 -
		((float)3 * Pram.PointStart.ddq - Pram.PointEnd.ddq)*t2) / ((float)2 * t3);
	DataO->B = ((float)30 * (Pram.PointStart.q - Pram.PointEnd.q) +
		((float)14 * Pram.PointEnd.dq + (float)16 * Pram.PointStart.dq)*t1 +
		((float)3 * Pram.PointStart.ddq - (float)2 * Pram.PointEnd.ddq)*t2) / ((float)2 * t4);
	DataO->A = ((float)12 * (Pram.PointEnd.q - Pram.PointStart.q) - 
		(float)6 * (Pram.PointEnd.dq + Pram.PointStart.dq)*t1 -
		(Pram.PointStart.ddq - Pram.PointEnd.ddq)*t2) / ((float)2 * t5);
    return 0;
}

int qcCalcPoint(STRUCT_QUINTIC_FACTOR Factor, float t, STRUCT_QUINTIC_POINT_BASE *Point)
{	
	float t1, t2, t3, t4, t5;
	t1 = t;
	t2 = t1*t1;
	t3 = t2*t1;
	t4 = t3*t1;
	t5 = t4*t1;
	Point->q = Factor.A*t5 + Factor.B*t4 + Factor.C*t3 + Factor.D*t2 + Factor.E*t1 + Factor.F;
	Point->dq = (float)5 * Factor.A*t4 + (float)4 * Factor.B*t3 + 
		(float)3 * Factor.C*t2 + (float)2 * Factor.D*t1 + Factor.E;
	Point->ddq = (float)20 * Factor.A*t3 + (float)12 * Factor.B*t2 + (float)6 * Factor.C*t + (float)2 * Factor.D;
	return 0;
}

